from libs.PipeLine import PipeLine, ScopedTiming
from libs.AIBase import AIBase
from libs.AI2D import Ai2d
import os
import ujson
from media.media import *
import nncase_runtime as nn
import ulab.numpy as np
import image
import gc
import sys
import aidemo
from machine import Pin
from machine import FPIOA
import time
import math

class PersonKeyPointApp(AIBase):
    # 保持原有代码不变
    def __init__(self, kmodel_path, model_input_size, confidence_threshold=0.1, nms_threshold=0.5, rgb888p_size=[1280, 720], display_size=[1920, 1080], debug_mode=0):
        super().__init__(kmodel_path, model_input_size, rgb888p_size, debug_mode)
        self.kmodel_path = kmodel_path
        self.model_input_size = model_input_size
        self.confidence_threshold = confidence_threshold
        self.nms_threshold = nms_threshold
        self.rgb888p_size = [ALIGN_UP(rgb888p_size[0], 16), rgb888p_size[1]]
        self.display_size = [ALIGN_UP(display_size[0], 16), display_size[1]]
        self.debug_mode = debug_mode
        self.SKELETON = [(16, 14), (14, 12), (17, 15), (15, 13), (12, 13), (6, 12), (7, 13), (6, 7), (6, 8), (7, 9), (8, 10), (9, 11), (2, 3), (1, 2), (1, 3), (2, 4), (3, 5), (4, 6), (5, 7)]
        self.LIMB_COLORS = [(255, 51, 153, 255), (255, 51, 153, 255), (255, 51, 153, 255), (255, 51, 153, 255), (255, 255, 51, 255), (255, 255, 51, 255), (255, 255, 51, 255), (255, 255, 128, 0), (255, 255, 128, 0), (255, 255, 128, 0), (255, 255, 128, 0), (255, 255, 128, 0), (255, 0, 255, 0), (255, 0, 255, 0), (255, 0, 255, 0), (255, 0, 255, 0), (255, 0, 255, 0), (255, 0, 255, 0), (255, 0, 255, 0)]
        self.KPS_COLORS = [(255, 0, 255, 0), (255, 0, 255, 0), (255, 0, 255, 0), (255, 0, 255, 0), (255, 0, 255, 0), (255, 255, 128, 0), (255, 255, 128, 0), (255, 255, 128, 0), (255, 255, 128, 0), (255, 255, 128, 0), (255, 255, 128, 0), (255, 51, 153, 255), (255, 51, 153, 255), (255, 51, 153, 255), (255, 51, 153, 255), (255, 51, 153, 255), (255, 51, 153, 255)]
        self.ai2d = Ai2d(debug_mode)
        self.ai2d.set_ai2d_dtype(nn.ai2d_format.NCHW_FMT, nn.ai2d_format.NCHW_FMT, np.uint8, np.uint8)

    def config_preprocess(self, input_image_size=None):
        with ScopedTiming("set preprocess config", self.debug_mode > 0):
            ai2d_input_size = input_image_size if input_image_size else self.rgb888p_size
            top, bottom, left, right = self.get_padding_param()
            self.ai2d.pad([0, 0, 0, 0, top, bottom, left, right], 0, [0, 0, 0])
            self.ai2d.resize(nn.interp_method.tf_bilinear, nn.interp_mode.half_pixel)
            self.ai2d.build([1, 3, ai2d_input_size[1], ai2d_input_size[0]], [1, 3, self.model_input_size[1], self.model_input_size[0]])

    def postprocess(self, results):
        with ScopedTiming("postprocess", self.debug_mode > 0):
            results = aidemo.person_kp_postprocess(results[0], [self.rgb888p_size[1], self.rgb888p_size[0]], self.model_input_size, self.confidence_threshold, self.nms_threshold)
            return results

    def draw_result(self, pl, res):
        with ScopedTiming("display_draw", self.debug_mode > 0):
            if res[0]:
                pl.osd_img.clear()
                kpses = res[1]
                for i in range(len(res[0])):
                    for k in range(17 + 2):
                        if k < 17:
                            kps_x, kps_y, kps_s = round(kpses[i][k][0]), round(kpses[i][k][1]), kpses[i][k][2]
                            kps_x1 = int(float(kps_x) * self.display_size[0] // self.rgb888p_size[0])
                            kps_y1 = int(float(kps_y) * self.display_size[1] // self.rgb888p_size[1])
                            if kps_s > 0:
                                pl.osd_img.draw_circle(kps_x1, kps_y1, 5, self.KPS_COLORS[k], 4)
                        ske = self.SKELETON[k]
                        pos1_x, pos1_y = round(kpses[i][ske[0]-1][0]), round(kpses[i][ske[0]-1][1])
                        pos1_x_ = int(float(pos1_x) * self.display_size[0] // self.rgb888p_size[0])
                        pos1_y_ = int(float(pos1_y) * self.display_size[1] // self.rgb888p_size[1])

                        pos2_x, pos2_y = round(kpses[i][(ske[1]-1)][0]), round(kpses[i][(ske[1]-1)][1])
                        pos2_x_ = int(float(pos2_x) * self.display_size[0] // self.rgb888p_size[0])
                        pos2_y_ = int(float(pos2_y) * self.display_size[1] // self.rgb888p_size[1])

                        pos1_s, pos2_s = kpses[i][(ske[0]-1)][2], kpses[i][(ske[1]-1)][2]
                        if pos1_s > 0.0 and pos2_s > 0.0:
                            pl.osd_img.draw_line(pos1_x_, pos1_y_, pos2_x_, pos2_y_, self.LIMB_COLORS[k], 4)
                    pl.osd_img.draw_circle(kps_x1, kps_y1, 3, (0, 255, 0), 2)  # 绿色圆心表示肩臀中点
                    gc.collect()
            else:
                pl.osd_img.clear()

    def get_padding_param(self):
        dst_w = self.model_input_size[0]
        dst_h = self.model_input_size[1]
        input_width = self.rgb888p_size[0]
        input_high = self.rgb888p_size[1]
        ratio_w = dst_w / input_width
        ratio_h = dst_h / input_high
        ratio = min(ratio_w, ratio_h)
        new_w = int(ratio * input_width)
        new_h = int(ratio * input_high)
        dw = (dst_w - new_w) // 2
        dh = (dst_h - new_h) // 2
        return dh, dh, dw, dw

class MultiBehaviorDetector:

    __slots__ = (
        'display_size', 
        'rgb888p_size', 
        'min_confidence', 
        'prev_positions', 
        'punch_history', 
        'punch_threshold', 
        'fall_slope_threshold', 
        'run_speed_threshold', 
        'crowd_distance_threshold', 
        'fight_speed_threshold', 
        'fight_accel_threshold', 
        'debug_mode', 
        'frame_count', 
        'fall_history'   
    )

    def __init__(self, display_size, rgb888p_size, debug_mode=3):  # 新增debug_mode参数
        self.display_size = display_size
        self.rgb888p_size = rgb888p_size
        self.min_confidence = 0.5 # 关键点置信度最小阈值
        self.prev_positions = {}  # 存储上一帧的位置
        self.punch_history = {}   # 存储挥拳历史
        self.fall_history = {}   # 初始化 fall_history
        self.punch_threshold = 3  # 连续挥拳次数阈值
        
        # 新增参数
        self.fall_slope_threshold = 1.0  # 跌倒斜率阈值
        self.run_speed_threshold = 1.2   # 奔跑速度阈值
        self.crowd_distance_threshold = 0.8  # 聚集距离阈值
        self.fight_speed_threshold = 100  # 打架速度阈值
        self.fight_accel_threshold = 10  # 打架加速度阈值
        
        # 新增：调试模式
        self.debug_mode = debug_mode
        self.frame_count = 0  # 帧计数器，用于调试
        
        
    def update_params(self, min_confidence=None, punch_threshold=None, 
                      fall_slope=None, run_speed=None, crowd_distance=None,
                      fight_speed=None, fight_accel=None):
        """更新MultiBehaviorDetector类的参数,增加参数校验"""
        if min_confidence is not None:
            if not (0 <= min_confidence <= 1):
                raise ValueError(f"min_confidence必须在[0,1]范围内，当前值: {min_confidence}")
            self.min_confidence = min_confidence
            
        if punch_threshold is not None:
            if punch_threshold < 1:
                raise ValueError(f"punch_threshold必须>=1, 当前值: {punch_threshold}")
            self.punch_threshold = punch_threshold
            
        # 其他参数校验...
        if fall_slope is not None:
            self.fall_slope_threshold = fall_slope
        if run_speed is not None:
            self.run_speed_threshold = run_speed
        if crowd_distance is not None:
            self.crowd_distance_threshold = crowd_distance
        if fight_speed is not None:
            self.fight_speed_threshold = fight_speed
        if fight_accel is not None:
            self.fight_accel_threshold = fight_accel
    
    # 新增：通用关键点置信度检查
    # indices：检测区域点
    def _check_keypoint_confidence(self, keypoints, indices):
        """检查指定索引的关键点置信度是否都达标"""
        for idx in indices:
            if keypoints[idx][2] < self.min_confidence:
                # if self.debug_mode > 1:
                #     print(f"关键点 {idx} 置信度不足: {keypoints[idx][2]} < {self.min_confidence}")
                return False
        return True
    
    # 判断是正视图还是侧视图
    def get_view_type(self, keypoints):
        """判断视图类型（正视图或侧视图）"""
        if not self._check_keypoint_confidence(keypoints, [5, 6]):  # 左右肩
            return None
            
        left_shoulder = keypoints[5]
        right_shoulder = keypoints[6]
        shoulder_distance = math.sqrt((right_shoulder[0] - left_shoulder[0])**2 + 
                                    (right_shoulder[1] - left_shoulder[1])**2)
        return ("front" if shoulder_distance > 50 else "side", shoulder_distance)

    def get_reference_distance(self, keypoints):
        """根据视图类型获取参考距离"""
        view_type = self.get_view_type(keypoints)
        if view_type[0] is None:
            raise ValueError(f"通过左右肩未检测出人物正视还是侧视图")
            return 0                              
        return view_type[1] * 2 if view_type[0] == "front" else view_type[1] * 1.5

    # 根据左右髋关节和膝关节判断
    def detect_run(self, keypoints, current_time):
        """改进版奔跑检测算法"""
        # 提取关键点并检查置信度
        if not self._check_keypoint_confidence(keypoints, [11, 12, 13, 14]):  # 左右髋、膝
            return False
            
        left_hip, right_hip = keypoints[11], keypoints[12]
        left_knee, right_knee = keypoints[13], keypoints[14]
        
        # 计算髋关节和膝关节的垂直距离
        left_hip_knee_dist = abs(left_knee[1] - left_hip[1])
        right_hip_knee_dist = abs(right_knee[1] - right_hip[1])

        
        # 计算身体移动速度
        person_id = hash((round((left_hip[0] + right_hip[0])/2), 
                        round((left_hip[1] + right_hip[1])/2)))
        
        # 使用髋关节中点作为参考点
        ref_point = ((left_hip[0] + right_hip[0])/2, (left_hip[1] + right_hip[1])/2)
        
        # 计算速度
        speed = self._calculate_speed(person_id, "body", ref_point, current_time)
        
        # 奔跑特征：
        # 1. 身体移动速度超过阈值
        # 2. 双腿关节垂直距离变化较大（跑步时膝盖会弯曲）
        is_running = speed > self.run_speed_threshold and \
                    (left_hip_knee_dist < 50 or right_hip_knee_dist < 50)
        
<<<<<<< HEAD
        # if self.debug_mode > 0 and is_running:
        #     print(f"检测到奔跑: 速度={speed:.2f}, 左膝距={left_hip_knee_dist}, 右膝距={right_hip_knee_dist}")
=======
        if self.debug_mode > 0 and is_running:
            print(f"检测到奔跑: 速度={speed:.2f}, 左膝距={left_hip_knee_dist}, 右膝距={right_hip_knee_dist}")
            time.sleep(10)
>>>>>>> 54a59364cc772039a799961f2ad471ddab3fd6c9
            
        return is_running

    def detect_crowd(self, all_keypoints, current_time):
        """改进版聚集检测算法（自适应优化）"""
        centers = []
    
        # 计算每个人的中心点和身体尺寸
        for kp in all_keypoints:
            # 检查关键点置信度
            if not self._check_keypoint_confidence(kp, [5, 6, 11, 12]):  # 肩、髋
                continue
            
            left_shoulder, right_shoulder = kp[5], kp[6]
            left_hip, right_hip = kp[11], kp[12]

            # 计算身体中心
            center_x = (left_shoulder[0] + right_shoulder[0] + left_hip[0] + right_hip[0]) / 4
            center_y = (left_shoulder[1] + right_shoulder[1] + left_hip[1] + right_hip[1]) / 4

            # 计算身体宽度和高度作为参考尺寸
            width = max(abs(right_shoulder[0] - left_shoulder[0]), 
                       abs(right_hip[0] - left_hip[0]))
            height = max(abs(right_shoulder[1] - right_hip[1]), 
                        abs(left_shoulder[1] - left_hip[1]))

            reference_size = (width + height) / 2

            centers.append((center_x, center_y, reference_size))
    
        n = len(centers)
        crowd_count = 0
    
        # 根据人数选择算法策略
        if n < 10:
            # 小规模场景：原始双重循环
            for i in range(n):
                for j in range(i + 1, n):
                    dx = centers[i][0] - centers[j][0]
                    dy = centers[i][1] - centers[j][1]

                    avg_size = (centers[i][2] + centers[j][2]) / 2
                
                    # 使用平方距离避免开方
                    threshold_sq = (avg_size * self.crowd_distance_threshold) ** 2
                    distance_sq = dx**2 + dy**2
                
  
                
                    if distance_sq < threshold_sq:
                        crowd_count += 1
    
        elif 10 <= n <= 30:
            # 中等规模：近似最近邻搜索（按x坐标排序）
            centers_sorted = sorted(centers, key=lambda c: c[0])
        
            for i, center_i in enumerate(centers_sorted):
                max_possible_dist = (center_i[2] + self.crowd_distance_threshold) * 2
            
                # 只检查x方向一定范围内的邻居
                for j in range(i + 1, n):
                    center_j = centers_sorted[j]
                
                    # 如果x距离已超过最大可能距离，提前终止
                    dx = center_i[0] - center_j[0]
                    if abs(dx) > max_possible_dist:
                        break
                
                    dy = center_i[1] - center_j[1]
                    if abs(dy) > max_possible_dist:
                        continue
                
                    # 使用平方距离比较
                    distance_sq = dx**2 + dy**2
                    avg_size = (center_i[2] + center_j[2]) / 2

                    threshold_sq = (avg_size * self.crowd_distance_threshold) ** 2
                
                    if distance_sq < threshold_sq:
                        crowd_count += 1
    
        else:  # n > 30
            # 大规模场景：网格空间分区
            # 计算动态网格大小（基于最大参考尺寸）
            max_size = max(c[2] for c in centers)
            grid_size = max_size * 2 * self.crowd_distance_threshold
        
            # 创建网格字典
            grid = {}
        
            # 将人员分配到网格
            for idx, (x, y, size) in enumerate(centers):
                grid_x = int(x // grid_size)
                grid_y = int(y // grid_size)
                grid_key = (grid_x, grid_y)
            
                if grid_key not in grid:
                    grid[grid_key] = []
                    grid[grid_key].append((x, y, size, idx))
        
            # 检查相邻网格中的人员
            for (gx, gy), cell in grid.items():
                # 检查当前网格和相邻的8个网格
                for dx in (-1, 0, 1):
                    for dy in (-1, 0, 1):
                        neighbor_key = (gx + dx, gy + dy)
                        neighbor_cell = grid.get(neighbor_key, [])
                    
                        # 比较当前网格和邻居网格中的人员
                        for person_i in cell:
                            for person_j in neighbor_cell:
                                # 确保只比较一次(i < j)
                                if person_i[3] >= person_j[3]:
                                    continue
                            
                            dx_val = person_i[0] - person_j[0]
                            dy_val = person_i[1] - person_j[1]
                            
                            # 使用平方距离比较
                            distance_sq = dx_val**2 + dy_val**2
                            avg_size = (person_i[2] + person_j[2]) / 2
                            threshold_sq = (avg_size * self.crowd_distance_threshold)**2
                            
                            if distance_sq < threshold_sq:
                                crowd_count += 1
    
            # 如果聚集对数超过一定阈值，认为是人群聚集
        is_crowd = crowd_count > n * 0.5
    
        if self.debug_mode > 0 and is_crowd:
            print(f"检测到人群聚集: {n}人中有{crowd_count}对近距离")
            print(f"使用算法: {'小规模' if n<10 else '中等规模' if n<=30 else '大规模'}")
        
        return is_crowd

    #  右肩、右髋、右脚
    def detect_fall(self, keypoints, current_time):
        """改进版跌倒检测算法"""
        # 定期清理历史数据
        if self.frame_count % 10 == 0:
            self._clean_history_fall(1.0)
    
        # 生成人员ID
        person_id = self._get_person_id(keypoints)
        if person_id is None:
            return False

        # 单帧跌倒检测
        current_fall = self._detect_fall_frame(keypoints, current_time)
    
        # 更新历史记录
        count, last_time = self.fall_history.get(person_id, (0, 0))
        if current_fall:
            count = min(count + 1, 5)  # 上限5
        else:
            count = max(count - 1, 0)  # 下限0
        self.fall_history[person_id] = (count, current_time)

        # 连续3帧检测到跌倒才确认
        return count >= 3

    def _detect_fall_frame(self, keypoints, current_time):
        """单帧跌倒检测优化"""
        # 优先检测右侧
        if self._check_keypoint_confidence(keypoints, [5, 6, 11, 12, 15, 16]):  # 肩、髋、脚踝
            if self._detect_side_fall(keypoints, "right", current_time):
                return True

        # 再检测左侧
        if self._check_keypoint_confidence(keypoints, [5, 6, 11, 12, 15, 16]):
            return self._detect_side_fall(keypoints, "left", current_time)
    
        return False

    def _detect_side_fall(self, keypoints, side, current_time):
        """单侧跌倒检测（优化版）"""
        # 确定关键点索引
        if side == "right":
            shoulder_idx, hip_idx, ankle_idx = 6, 12, 16  # 右肩、右髋、右脚踝
            opposite_shoulder_idx = 5
        else:  # left
            shoulder_idx, hip_idx, ankle_idx = 5, 11, 15  # 左肩、左髋、左脚踝
            opposite_shoulder_idx = 6
    
        # 提取关键点
        shoulder = keypoints[shoulder_idx]
        hip = keypoints[hip_idx]
        ankle = keypoints[ankle_idx]
        opposite_shoulder = keypoints[opposite_shoulder_idx]

        # 提取坐标
        s_x, s_y = shoulder[0], shoulder[1]
        h_x, h_y = hip[0], hip[1]
        a_x, a_y = ankle[0], ankle[1]
        os_x, os_y = opposite_shoulder[0], opposite_shoulder[1]

        # 1. 计算躯干角度（肩到髋）
        dx_sh = h_x - s_x
        dy_sh = h_y - s_y
        torso_angle = math.atan2(dy_sh, dx_sh) * 180 / math.pi  # 转换为角度
    
        # 2. 计算腿部角度（髋到脚踝）
        dx_ha = a_x - h_x
        dy_ha = a_y - h_y
        leg_angle = math.atan2(dy_ha, dx_ha) * 180 / math.pi
    
        # 3. 计算肩宽
        shoulder_width = abs(os_x - s_x)
    
        # 4. 计算髋部高度差（用于检测快速下降）
        person_id = self._get_person_id(keypoints)
        hip_speed = 0
        if person_id is not None:
            hip_speed = self._calculate_speed(
                person_id, 
                "hip", 
                (h_x, h_y),
                current_time
            )
    
        # 跌倒判断条件：
        # a. 躯干接近水平（±30度内）
        is_torso_horizontal = abs(torso_angle) < 30 or abs(torso_angle - 180) < 30
    
        # b. 腿部接近水平（±30度内）
        is_leg_horizontal = abs(leg_angle) < 30 or abs(leg_angle - 180) < 30
    
        # c. 髋部快速下降
        is_fast_fall = hip_speed > 15  # 降低阈值到15像素/秒
    
        # d. 身体高度显著降低（肩到脚踝的距离小于肩宽的1.5倍）
        body_height = abs(a_y - s_y)
        is_low_position = body_height < shoulder_width * 1.5
    
        # 组合条件：满足任意两个条件即认为可能跌倒
        condition_count = 0
        if is_torso_horizontal: condition_count += 1
        if is_leg_horizontal: condition_count += 1
        if is_fast_fall: condition_count += 1
        if is_low_position: condition_count += 1
    
        # 调试输出
        # if self.debug_mode > 1 and condition_count >= 2:
        #     print(f"{side}侧跌倒检测:")
        #     print(f"  躯干角度: {torso_angle:.1f}° ({'水平' if is_torso_horizontal else '垂直'})")
        #     print(f"  腿部角度: {leg_angle:.1f}° ({'水平' if is_leg_horizontal else '垂直'})")
        #     print(f"  髋部速度: {hip_speed:.1f} px/s ({'快速' if is_fast_fall else '正常'})")
        #     print(f"  身体高度: {body_height:.1f} < {shoulder_width*1.5:.1f} ({'低' if is_low_position else '高'})")
        #     print(f"  满足条件数: {condition_count}/4")
            

        # 这里改为满足一个条件及跌倒
        return condition_count >= 2

    def _get_person_id(self, keypoints):
        """生成稳定的人员ID"""
        # 使用肩膀位置生成ID（更稳定的方式）
        shoulder_left = keypoints[5]
        shoulder_right = keypoints[6]

        # 使用整数坐标生成ID（避免浮点数精度问题）
        id_x = int((shoulder_left[0] + shoulder_right[0]) * 50)  # 放大坐标
        id_y = int((shoulder_left[1] + shoulder_right[1]) * 50)
        return (id_x << 16) | id_y  # 组合成32位整数ID 

    def _clean_history_fall(self, max_age=1.0):
        """专用跌倒历史清理"""
        current_time = time.time()
        keys_to_delete = []

        for person_id, (count, last_time) in self.fall_history.items():
            if current_time - last_time > max_age:
                keys_to_delete.append(person_id)

        for person_id in keys_to_delete:
            try:
                del self.fall_history[person_id]
            except KeyError:
                pass

    def detect_fight(self, all_keypoints, current_time):
        """改进版打架检测算法"""
        if len(all_keypoints) < 2:
            return False

        fight_score = 0
        person_count = 0
    
        for i, kp in enumerate(all_keypoints):
            # 跳过置信度不足的人
            if not self._check_keypoint_confidence(kp, [5, 6]):  # 双肩
                continue
            
            person_count += 1
        
            # 检测挥拳动作（改进版）
            if not self._check_keypoint_confidence(kp, [5, 6, 7, 8, 9, 10]):  # 肩、肘、腕
                continue
            
            left_shoulder, right_shoulder = kp[5], kp[6]
            left_elbow, right_elbow = kp[7], kp[8]
            left_wrist, right_wrist = kp[9], kp[10]
        
            # 计算挥拳速度（改进版：考虑三维方向）
            person_id = hash((round(left_shoulder[0]), round(left_shoulder[1])))
        
            # 计算左手和右手的速度和加速度
            left_hand_speed = self._calculate_speed(person_id, "left_hand", 
                                                 (left_wrist[0], left_wrist[1]), current_time)
            right_hand_speed = self._calculate_speed(person_id, "right_hand", 
                                                  (right_wrist[0], right_wrist[1]), current_time)
        
            left_hand_accel = self._calculate_acceleration(person_id, "left_hand", 
                                                          left_hand_speed, current_time)
            right_hand_accel = self._calculate_acceleration(person_id, "right_hand", 
                                                           right_hand_speed, current_time)
        
            # 检测挥拳动作（多方向）
            left_punch = self._detect_punch(person_id, "left", 
                                           left_shoulder, left_elbow, left_wrist,
                                           left_hand_speed, left_hand_accel, current_time)
        
            right_punch = self._detect_punch(person_id, "right", 
                                            right_shoulder, right_elbow, right_wrist,
                                            right_hand_speed, right_hand_accel, current_time)
        
            # 更新挥拳历史
            if person_id not in self.punch_history:
                self.punch_history[person_id] = {"left": 0, "right": 0}
            
            if left_punch:
                self.punch_history[person_id]["left"] += 1
            else:
                self.punch_history[person_id]["left"] = max(0, self.punch_history[person_id]["left"] - 1)
            
            if right_punch:
                self.punch_history[person_id]["right"] += 1
            else:
                self.punch_history[person_id]["right"] = max(0, self.punch_history[person_id]["right"] - 1)
            
            # 如果连续多次挥拳，增加打架分数
            if self.punch_history[person_id]["left"] >= self.punch_threshold or \
               self.punch_history[person_id]["right"] >= self.punch_threshold:
                fight_score += 1
            
            # 检测与其他人的身体对抗（改进版）
            for j in range(i + 1, len(all_keypoints)):
                other_kp = all_keypoints[j]
                if not self._check_keypoint_confidence(other_kp, [5, 6]):  # 对方双肩
                    continue
                
                # 计算两个人的身体对抗程度
                conflict_level = self._calculate_conflict_level(kp, other_kp)
            
                # 如果有身体对抗且有挥拳动作，增加打架分数
                if conflict_level > 0.5 and (left_punch or right_punch):
                   fight_score += conflict_level
                    
        # 如果有足够多的人参与打架动作，判断为打架
        is_fight = fight_score >= 1.5 and person_count >= 2
    
        # if self.debug_mode > 0 and is_fight:
        #     print(f"检测到打架: {person_count}人参与，打架分数={fight_score}")
        
        return is_fight
    
    def _detect_punch(self, person_id, hand_side, shoulder, elbow, wrist, speed, accel, current_time):
        """检测挥拳动作（多方向）"""
        # 基础速度和加速度检查
        if speed < self.fight_speed_threshold or accel < self.fight_accel_threshold:
            return False
            
        # 计算手臂角度
        def calculate_angle(a, b, c):
            # 计算向量
            vector_ab = (b[0] - a[0], b[1] - a[1])
            vector_bc = (c[0] - b[0], c[1] - b[1])
            
            # 计算点积
            dot_product = vector_ab[0] * vector_bc[0] + vector_ab[1] * vector_bc[1]
            
            # 计算向量长度
            len_ab = math.sqrt(vector_ab[0]**2 + vector_ab[1]**2)
            len_bc = math.sqrt(vector_bc[0]**2 + vector_bc[1]**2)
            
            # 避免除零错误
            if len_ab == 0 or len_bc == 0:
                return 0
                
            # 计算角度（弧度）
            cos_angle = dot_product / (len_ab * len_bc)
            cos_angle = max(-1, min(1, cos_angle))  # 防止数值误差导致超出范围
            return math.acos(cos_angle)
            
        current_angle = calculate_angle((shoulder[0], shoulder[1]), 
                                    (elbow[0], elbow[1]), 
                                    (wrist[0], wrist[1]))
                                    
        # 检查角度变化历史
        angle_key = f"{person_id}_{hand_side}_angle"
        
        # 确保角度数据格式正确
        if angle_key in self.prev_positions:
            prev_data = self.prev_positions[angle_key]
            # 验证数据格式
            if not isinstance(prev_data, (list, tuple)) or len(prev_data) != 2:
                print(f"警告: {angle_key}格式错误，重置数据")
                self.prev_positions[angle_key] = (current_angle, current_time)
                return False
                
            prev_angle, prev_time = prev_data
            angle_change = abs(current_angle - prev_angle)
            
            # 角度快速变化表示挥拳动作
            if angle_change > 0.5:  # 约28.6度
                self.prev_positions[angle_key] = (current_angle, current_time)
                return True
                
        # 首次存储角度数据
        self.prev_positions[angle_key] = (current_angle, current_time)
        return False    
    def _calculate_conflict_level(self, kp1, kp2):
        """计算两个人的身体对抗程度"""
        # 计算身体重心
        def get_center(kp):
            if not self._check_keypoint_confidence(kp, [5, 6, 11, 12]):  # 肩、髋
                return None
                
            left_shoulder, right_shoulder = kp[5], kp[6]
            left_hip, right_hip = kp[11], kp[12]
            
            center_x = (left_shoulder[0] + right_shoulder[0] + left_hip[0] + right_hip[0]) / 4
            center_y = (left_shoulder[1] + right_shoulder[1] + left_hip[1] + right_hip[1]) / 4
            
            return (center_x, center_y)
            
        center1 = get_center(kp1)
        center2 = get_center(kp2)
        
        if not center1 or not center2:
            return 0
            
        # 计算重心距离
        dx = center2[0] - center1[0]
        dy = center2[1] - center1[1]
        distance = math.sqrt(dx**2 + dy**2)
        
        # 计算身体宽度作为参考
        def get_body_width(kp):
            if not self._check_keypoint_confidence(kp, [5, 6]):  # 双肩
                return 1
                
            left_shoulder, right_shoulder = kp[5], kp[6]
            width = abs(right_shoulder[0] - left_shoulder[0])
            return max(width, 10)  # 确保最小宽度为10，防止除零
            
        width1 = get_body_width(kp1)
        width2 = get_body_width(kp2)
        avg_width = (width1 + width2) / 2
        
        # 防止除零错误（修复原代码中的问题）
        if avg_width <= 0:
            # if self.debug_mode > 1:
            #     print(f"警告: 平均宽度为{avg_width}，可能导致除零错误")
            return 0
            
        # 距离越近，对抗程度越高
        conflict_level = max(0, 1 - distance / (avg_width * 1.5))
        
        # 检查手部是否接触对方身体
        def check_hand_contact(person_kp, other_kp, other_center):
            # 检查左手是否接触对方
            if person_kp[9][2] > self.min_confidence:  # 左手腕
                left_wrist = person_kp[9]
                # 计算左手腕到对方重心的距离
                dist = math.sqrt((left_wrist[0] - other_center[0])**2 + 
                               (left_wrist[1] - other_center[1])**2)
                if dist < avg_width * 0.6:
                    return True
                    
            # 检查右手是否接触对方
            if person_kp[10][2] > self.min_confidence:  # 右手腕
                right_wrist = person_kp[10]
                dist = math.sqrt((right_wrist[0] - other_center[0])**2 + 
                               (right_wrist[1] - other_center[1])**2)
                if dist < avg_width * 0.6:
                    return True
            return False
            
        # 如果有手部接触，增加对抗程度
        if check_hand_contact(kp1, kp2, center2) or check_hand_contact(kp2, kp1, center1):
            conflict_level = min(1.0, conflict_level + 0.3)
            
        # if self.debug_mode > 1:
        #     print(f"身体对抗程度: 距离={distance:.2f}, 平均宽度={avg_width}, 冲突级别={conflict_level:.2f}")
            
        return conflict_level
    
    def _calculate_speed(self, person_id, part_name, position, current_time):
        """计算关键点移动速度"""
        key = f"{person_id}_{part_name}"
        
        # 检查并修复可能的错误数据
        if key in self.prev_positions:
            prev_data = self.prev_positions[key]
            if not isinstance(prev_data, (list, tuple)) or len(prev_data) != 3:
                print(f"警告: {key}格式错误，重置数据")
                self.prev_positions[key] = (position[0], position[1], current_time)
                return 0
                
        # 计算速度
        if key in self.prev_positions:
            prev_x, prev_y, prev_time = self.prev_positions[key]
            dx = position[0] - prev_x
            dy = position[1] - prev_y
            dt = current_time - prev_time
            if dt > 0:
                speed = math.sqrt(dx**2 + dy**2) / dt
                self.prev_positions[key] = (position[0], position[1], current_time)
                return speed
                
        # 首次存储位置数据
        self.prev_positions[key] = (position[0], position[1], current_time)
        return 0

    def _calculate_acceleration(self, person_id, part_name, current_speed, current_time):
        """计算加速度"""
        key = f"{person_id}_{part_name}_speed"
        
        # 检查并修复可能的错误数据
        if key in self.prev_positions:
            prev_data = self.prev_positions[key]
            if not isinstance(prev_data, (list, tuple)) or len(prev_data) != 2:
                print(f"警告: {key}格式错误，重置数据")
                self.prev_positions[key] = (current_speed, current_time)
                return 0
                
        # 计算加速度
        if key in self.prev_positions:
            prev_speed, prev_time = self.prev_positions[key]
            dt = current_time - prev_time
            if dt > 0:
                accel = abs(current_speed - prev_speed) / dt
                self.prev_positions[key] = (current_speed, current_time)
                return accel
                
        # 首次存储速度数据
        self.prev_positions[key] = (current_speed, current_time)
        return 0    
    def clean_history(self, max_age=1.0):
        """增强版历史数据清理，支持二元组（速度/角度）和三元组（位置）格式"""
        current_time = time.time()
        keys_to_delete = []
        
        for key in list(self.prev_positions.keys()):
            try:
                value = self.prev_positions[key]
                # 检查值是否为序列
                if not isinstance(value, (list, tuple)):
                    print(f"清理历史数据时出错 - Key: {key}, 错误: 值不是序列类型，类型为 {type(value)}")
                    keys_to_delete.append(key)
                    continue
                    
                # 区分角度相关键（结尾含_angle）、速度相关键（结尾含_speed）和位置相关键
                is_angle_key = "_angle" in key
                is_speed_key = "_speed" in key
                
                # 获取时间戳
                if is_angle_key or is_speed_key:
                    # 角度/速度数据应为 (value, timestamp) 二元组
                    if len(value) != 2:
                        print(f"警告: {key}数据长度异常: {len(value)}，预期2")
                        keys_to_delete.append(key)
                        continue
                    timestamp = value[1]  # 时间戳在索引1
                else:
                    # 位置数据应为 (x, y, timestamp) 三元组
                    if len(value) != 3:
                        print(f"警告: {key}数据长度异常: {len(value)}，预期3")
                        keys_to_delete.append(key)
                        continue
                    timestamp = value[2]  # 时间戳在索引2
                
                # 校验时间戳类型
                if not isinstance(timestamp, (int, float)):
                    print(f"警告: {key}时间戳类型错误: {type(timestamp)}")
                    keys_to_delete.append(key)
                    continue
                    
                # 判断是否过期
                if current_time - timestamp > max_age:
                    keys_to_delete.append(key)
                    
            except Exception as e:
                print(f"清理历史数据时发生意外错误 - Key: {key}, 错误: {str(e)}")
                keys_to_delete.append(key)
        
        # 安全删除过期键
        for key in keys_to_delete:
            try:
                # print(f"清理前: 历史数据大小={len(self.prev_positions)}")
                del self.prev_positions[key]
                # print(f"清理后: 历史数据大小={len(self.prev_positions)}")
            except KeyError:
                pass


def init_ai_model(kmodel_path, rgb888p_size, display_size):
    person_kp = PersonKeyPointApp(
        kmodel_path,
        model_input_size=[320, 320],
        confidence_threshold=0.1,
        nms_threshold=0.5,
        rgb888p_size=rgb888p_size,
        display_size=display_size
    )
    person_kp.config_preprocess()
    
    detector = MultiBehaviorDetector(display_size, rgb888p_size)
    return person_kp, detector

# 执行AI推理
def run_inference(person_kp, detector, pl, current_time):
    img = pl.get_frame()
    res = person_kp.run(img)

    detection_results = {
        "fall": False,
        "run": False,
        "crowd": False,
        "fight": False  # 新增打架检测结果
    }

    if res[0] and len(res[1]) > 0:
        all_keypoints = res[1]

        # 单人检测
        for keypoints in all_keypoints:
            if detector.detect_fall(keypoints, current_time):
                detection_results["fall"] = True
                print("⚠️ 检测到跌倒")
                time.sleep(3)
                
            if detector.detect_run(keypoints, current_time):
                detection_results["run"] = True
                print("⚠️ 检测到奔跑")

        # 多人检测
        if len(all_keypoints) >= 2:
            if detector.detect_crowd(all_keypoints, current_time):
                detection_results["crowd"] = True
                print("⚠️ 检测到聚集")
            if detector.detect_fight(all_keypoints, current_time):  
                detection_results["fight"] = True
                print("⚠️ 检测到打架")
    # print(f"剩余内存: {gc.mem_free()}")
    person_kp.draw_result(pl, res)
    detector.frame_count = (detector.frame_count + 1) % 10
    if detector.frame_count == 0:
        detector.clean_history()
    return detection_results